package gu.ai;

import gu.common.Arrays;
import gu.struct.BinaryHeap;

public final class Astar {

	private final static int OPEN_BIT = 0;
	private final static int CLOSE_BIT = 1;
	
	public int[] costMap;
	public int[] stateMap;
	public BinaryHeap fRank;
	
	public int start;
	public int end;
	
	public int w,h;
	
	public int value;
	
	private final void init(int[] map, int w, int h) {
		costMap = map;
		
		if(stateMap == null || this.w != w || this.h !=h)
			stateMap = new int[w * h];
		else {
			Arrays.fill(stateMap, 0);
		}
		
		fRank = new BinaryHeap(w * h);
		this.w = w;
		this.h = h;
	}
	
	private final int predictDistance(int start, int end) {
		int sx = start % w;
		int sy = start / w;
		
		int ex = end % w;
		int ey = end / w;
		
		return (int)Math.floor(Math.sqrt((sx - ex) * (sx - ex) + (sy - ey) * (sy - ey))) * 10;
	}
		
	public short[] findPath(int[] map, int w, int h, int start, int end) {
		init(map, w, h);
		
		this.start = start;
		this.end = end;
		
		addToOpen(start);

		addToFRank(start, 0);
		
		short idx;
		
		while(true) {
			/*
			System.out.print("frank: count = " + fRank.count + " ");
			fRank.print();
			printStateMap();
			*/
			idx = getBestIndex();
			
			//System.out.println("best idx = " + idx);
			
			if(idx < 0)
				return null;

			if(idx == end)
				return makePath();

			removeBestFRank();
			
			if(decodeIsOnClose(stateMap[idx])) 
				continue;
			
			removeFromOpen(idx);
			addToClose(idx);

			if(idx % w != 0) {
				process(idx - 1, idx, 10);
				process(idx - 1 - w, idx, 14);
				process(idx - 1 + w, idx, 14);
			}
			
			if(idx % w != w - 1) {
				process(idx + 1, idx, 10);
				process(idx + 1 - w, idx, 14);
				process(idx + 1 + w, idx, 14);
			}
			
			process(idx + w, idx, 10);
			process(idx - w, idx, 10);
		}
		
	}
	
	private short[] makePath() {
		short[] path = new short[w*h];
		
		int i = 0;
		
		short idx = (short)end;
		
		path[i++] = (short)end;
		
		while(idx != start) {
			idx = decodeParent(stateMap[idx]);
			path[i++] = idx;
		}
		
		path[i++] = (short)start;

		return Arrays.copyOf(path, i);
	}
	
	private final void process(int idx, short parent, int cost) {
		
		if(idx < 0 || idx >= stateMap.length) {
			return;
		}

		if(costMap[idx] < 0)
			return;
		
		value = stateMap[idx];
		
		if(decodeIsOnClose(value)) 
			return;
		
		int parentG = decodeG(stateMap[parent]);
		int newG = parentG + cost;
		short newParent = parent;
		
		int h = predictDistance(idx, end) * 10 + costMap[idx];
		
		int f;
		
		if(decodeIsOnOpen(value)) {
			int currentG = decodeG(value);
			
			if(currentG > newG) {
				f = newG + h;
				replaceFRank(idx, fRank.getFirst() >> 16, f);
				setParent(idx, newParent);
				setG(idx, newG);
			}
			
		} else {
			f = newG + h;
			addToOpen(idx);
			addToFRank(idx, f);
			setParent(idx, newParent);
			setG(idx, newG);
		}


	}
	
	private final void addToOpen(int idx) {
		stateMap[idx] = stateMap[idx] | (1 << OPEN_BIT);
	}

	private final void removeFromOpen(int idx) {
		int value = stateMap[idx];
		boolean c = decodeIsOnClose(value);
		short p = decodeParent(value);
		short g = decodeG(value);
		stateMap[idx] = encode(false, c, p, g);
	}
	
	private final void addToClose(int idx) {
		stateMap[idx] = stateMap[idx] | (1 << CLOSE_BIT);
	}
	
	private final void setParent(int idx, short parent) {
		int value = stateMap[idx];
		stateMap[idx] = encode(decodeIsOnOpen(value), decodeIsOnClose(value), parent, decodeG(value)); 
	}
	
	private final void setG(int idx, int g) {
		int value = stateMap[idx];
		stateMap[idx] = encode(decodeIsOnOpen(value), decodeIsOnClose(value), decodeParent(value), g); 
	}

	
	public final static boolean decodeIsOnOpen(int value) {
		return (value & 1 << OPEN_BIT) == 1 << OPEN_BIT;
	}

	public final static boolean decodeIsOnClose(int value) {
		return (value & 1 << CLOSE_BIT) == 1 << CLOSE_BIT;
	}

	public final static short decodeParent(int value) {
		return (short)((value << 16) >> 18);
	}
	
	public final static short decodeG(int value) {
		return (short)(value >> 16);
	}
	
	public final static int encode(boolean onOpen, boolean onClose, short parent, int g) {
		g = g << 16;
		g = g | (parent << 2);
		g = g | ((onClose ? 1 : 0) << CLOSE_BIT);
		g = g | ((onOpen ? 1 : 0) << OPEN_BIT);
		return g;
	}
	
	private final void addToFRank(int ind, int f) {
		fRank.add(f << 16 | ind);
	}
	
	private final void replaceFRank(int ind, int oldF, int newF) {
		fRank.replaceWithBetter(oldF << 16 | ind, newF << 16 | ind);
	}

	private final void removeBestFRank() {
//		System.out.println("removing " + fRank.getFirst());
		fRank.pop();
	}
	
	private final short getBestIndex() {
		if(fRank.isEmpty())
			return -1;
		else
			return (short)((fRank.getFirst() << 16) >> 16);
	}
	
	public final static void printMap(int[] map, int w, int h) {
		for(int i=0;i<h;i++) {
			
			for(int j=0;j<w;j++) {
				System.out.print(map[i*w + j] + " ");
			}
			
			System.out.println("");
		}
	}
	
	public final void printStateMap() {
		for(int i=0;i<h;i++) {
			
			for(int j=0;j<w;j++) {
				printCell(stateMap[i * w + j]);
				System.out.print(" ");
			}
			
			System.out.println("");
		}
	}
	
	public final void printCell(int value) {
		boolean o = decodeIsOnOpen(value);
		boolean c = decodeIsOnClose(value);
		int g = decodeG(value);
		int p = decodeParent(value);
		System.out.print(" ");
		System.out.print(o ? "o" : "_");
		System.out.print(c ? "c" : "_");
		System.out.print(p);
		System.out.print("-" + g);
	}
	
}
